#include "utils.h"

using namespace fvision;

CObservationImageLandmarks predictImageLandmarks(const Camera1D& camera, const LandmarkMap& map) {

	CObservationImageLandmarks observation;

	for (int i = 0; i < map.landmarks.size(); i++) {
		int id = map.landmarks[i].first;
		CvPoint2D32f p = map.landmarks[i].second;
		double ix = camera.calculateImagePoint(p);
		if (camera.isPoint1DInImage(ix)) {
			CObservationImageLandmarks::TMeasurement measurement;
			measurement.id = id;
			measurement.x  = ix;
			observation.sensedData.push_back(measurement);
		}
	}

	return observation;
}